
% 初始化
clear,clc;
t=(0.01:0.01:10)';

% 定义系统参数
Rr = 0.0013;
Xm = 2.6;
Xrr = Xm;
Ts = 0.01;
C = 13.29;
Tf = 3.4;
D = 5;
J = 6.5;
R = 0.0013;
Xs = 2.9;
omega_s = 1.0;

X_sigma_r = 2.9;
X_m = 2.6;
X_dr = X_sigma_r + X_m;
X_qr = X_sigma_r + X_m;

% 初始化变量
N = 1000;
E_d = zeros(N,1);
E_q = zeros(N,1);
U_dc = zeros(N,1);
I_dr = zeros(N,1);
I_qr = zeros(N,1);
omega_r = zeros(N,1);
U_qs = zeros(N,1);
U_ds = zeros(N,1);
U_dr = zeros(N,1);
U_qr = zeros(N,1);
T_m = zeros(N,1);
T_e = zeros(N,1);

% 定义输入变量
t = (1:N) * Ts;
I_dl = 0.5 * sin(2 * pi * 0.1 * (1:N) * Ts);
U_qs = 1.0 * sin(2 * pi * 50 * t);
U_ds = 0.5 * cos(2 * pi * 50 * t);
U_dr = 0.2 * sin(2 * pi * 50 * t);
U_qr = 0.1 * cos(2 * pi * 50 * t);
T_m = 0.8 + 0.1 * sin(2 * pi * 50 * t);
T_e = 0.6 + 0.05 * cos(2 * pi * 50 * t);

% 定义调制信号
m_d = 0.8 * sin(2 * pi * 50 * t);
m_q = 0.5 * cos(2 * pi * 50 * t);

% 生成真实的x与观测z
n = 6;
m = 6;
x = zeros(n,1000);
z = zeros(m,1000);
x(1,1) = 0;
x(2,1) = 0;
x(3,1) = 0;
x(4,1) = 0;
x(5,1) = 0;
x(6,1) = 0;

X = zeros(n,1000);
X(1,1) = 0;
X(2,1) = 0;
X(3,1) = 0;
X(4,1) = 0;
X(5,1) = 0;
X(6,1) = 0;

RMSE = zeros(1,1000);
MSE = zeros(1,1000);
MAE = zeros(1,1000);

Pplus = eye(6)*1;
Q = eye(6)*1;
Rs = eye(6)*1;
Py = zeros(n,n);
Pxy = zeros(n,n);

% LSTM参数
numFeatures = 12;
numResponses = 6;
numHiddenUnits = 100;
numTimeSteps = 1000;

layers = [sequenceInputLayer(numFeatures)
    lstmLayer(numHiddenUnits)
    dropoutLayer(0.5)
    fullyConnectedLayer(numResponses)
    regressionLayer];

MaxEpochs = 300;
InitialLearnRate = 0.01;

% 初始化UKF变量
w = zeros(n,2*n+1);
lamda = 21;
for i = 1:2*n+1
    w(i) = 1/(2*(n+lamda));
end
w(1) = lamda/(n+lamda);

% 主循环
for i = 2:numTimeSteps
    % 更新d轴暂态电势
    x(1,i) = x(1,i-1) + Ts * ( (Rr/Xrr)*(U_qs(i-1) - x(2,i-1)) -  x(6,i-1) * x(1,i-1) );
    % 更新q轴暂态电势
    x(2,i)= x(2,i-1) + Ts * ( (Rr/Xrr)*(U_ds(i-1) - x(1,i-1)) +  x(6,i-1)* x(2,i-1) );
    % 更新直流电压
    x(3,i) = x(3,i-1) + Ts * ( C/Tf * (x(4,i-1) - I_dl(i-1)) );
    % 更新转子d轴电流
    x(4,i) = x(4,i-1)+ Ts * ( (1/X_dr) * (U_dr(i-1) - Rr * x(4,i-1)) );
    % 更新转子q轴电流
    x(5,i) = x(5,i-1) + Ts * ( (1/X_qr) * (U_qr(i-1) - Rr * x(5,i-1)) );
    % 更新转子角速度
    x(6,i)=  x(6,i-1)+ Ts * ( (T_m(i-1) - T_e(i-1) - D *  x(6,i-1)) / J );
    
    U_d(i-1) = m_d(i-1) * U_dc(i-1) / 2;
    U_q(i-1) = m_q(i-1) * U_dc(i-1) / 2;
    
    z(1,i-1)=(omega_s*Xs)/(R+Xs) * x(2,i-1)+ (R/(R+Xs)) * U_q(i-1);
    z(2,i-1)=(omega_s*Xs)/(R+Xs) * x(1,i-1) - (R/(R+Xs)) * U_d(i-1);
    z(3,i-1)=x(3,i-1);
    z(4,i-1)=(Xs/(R+Xs)) * x(3,i-1) - (R/(R+Xs)) * x(1,i-1)+ (omega_s*Xs)/(R+Xs) * z(1,i);
    z(5,i-1)=(R/(R+Xs)) * x(3,i-1) + (Xs/(R+Xs)) * x(1,i-1) + (omega_s*R)/(R+Xs) * z(2,i);
    z(6,i-1)=x(6,i-1);
    
    % --- UKF算法实体 ---
    % 1. 生成Sigma点
    x_hat = X(:,i-1);
    P = Pplus;
    sigma_points = zeros(n, 2*n+1);
    sigma_points(:,1) = x_hat;
    sqrtP = chol((n + lamda)*P)';
    for j = 1:n
        sigma_points(:,j+1) = x_hat + sqrtP(:,j);
        sigma_points(:,n+j+1) = x_hat - sqrtP(:,j);
    end
    
    % 2. 预测步骤（状态转移）
    Xsigminus = zeros(n, 2*n+1);
    for j = 1:2*n+1
        x_j = sigma_points(:,j);
        E_d_next = x_j(1) + Ts*((Rr/Xrr)*(U_qs(i-1) - x_j(2)) - x_j(6)*x_j(1));
        E_q_next = x_j(2) + Ts*((Rr/Xrr)*(U_ds(i-1) - x_j(1)) + x_j(6)*x_j(2));
        U_dc_next = x_j(3) + Ts*(C/Tf*(x_j(4) - I_dl(i-1)));
        I_dr_next = x_j(4) + Ts*((1/X_dr)*(U_dr(i-1) - Rr*x_j(4)));
        I_qr_next = x_j(5) + Ts*((1/X_qr)*(U_qr(i-1) - Rr*x_j(5)));
        omega_r_next = x_j(6) + Ts*((T_m(i-1) - T_e(i-1) - D*x_j(6))/J);
        
        Xsigminus(:,j) = [E_d_next; E_q_next; U_dc_next; I_dr_next; I_qr_next; omega_r_next];
    end
    
    % 计算预测均值和协方差
    Xminus = zeros(n,1);
    for j = 1:2*n+1
        Xminus = Xminus + w(j)*Xsigminus(:,j);
    end
    Pminus = Q;
    for j = 1:2*n+1
        diff = Xsigminus(:,j) - Xminus;
        Pminus = Pminus + w(j)*(diff*diff');
    end
    
    % 3. 观测预测
    Zsigma = zeros(m, 2*n+1);
    for j = 1:2*n+1
        x_j = Xsigminus(:,j);
        U_d_sigma = m_d(i) * x_j(3) / 2;
        U_q_sigma = m_q(i) * x_j(3) / 2;
        z1 = (omega_s*Xs)/(R+Xs)*x_j(2) + (R/(R+Xs))*U_q_sigma;
        z2 = (omega_s*Xs)/(R+Xs)*x_j(1) - (R/(R+Xs))*U_d_sigma;
        z3 = x_j(3);
        z4 = (Xs/(R+Xs))*x_j(3) - (R/(R+Xs))*x_j(1) + (omega_s*Xs)/(R+Xs)*z1;
        z5 = (R/(R+Xs))*x_j(3) + (Xs/(R+Xs))*x_j(1) + (omega_s*R)/(R+Xs)*z2;
        z6 = x_j(6);
        Zsigma(:,j) = [z1; z2; z3; z4; z5; z6];
    end
    
    % 计算观测均值和协方差
    Zminus = zeros(m,1);
    for j = 1:2*n+1
        Zminus = Zminus + w(j)*Zsigma(:,j);
    end
    Pz = Rs;
    for j = 1:2*n+1
        diff_z = Zsigma(:,j) - Zminus;
        Pz = Pz + w(j)*(diff_z*diff_z');
    end
    
    % 计算互协方差
    Pxz = zeros(n,m);
    for j = 1:2*n+1
        diff_x = Xsigminus(:,j) - Xminus;
        diff_z = Zsigma(:,j) - Zminus;
        Pxz = Pxz + w(j)*(diff_x*diff_z');
    end
    
    % 4. 卡尔曼增益和更新
    K = Pxz / Pz;
    X(:,i) = Xminus + K*(z(:,i) - Zminus);
    Pplus = Pminus - K*Pz*K';
    
    % LSTM嵌入UKF循环内训练
    data = [X(:,i-1); x(:,i-1)];
    XTrain = data(:);
    YTrain = x(:,i);
    
    mu = mean(XTrain,'ALL');
    sig = std(XTrain,0,'ALL');
    XTrain = (XTrain - mu)/sig;
    YTrain = (YTrain - mu)/sig;
    
    options = trainingOptions('adam',...
        'MaxEpochs',MaxEpochs,...
        'MiniBatchSize',128,...
        'GradientThreshold',1,...
        'InitialLearnRate',InitialLearnRate,...
        'LearnRateSchedule','piecewise',...
        'LearnRateDropPeriod',100,...
        'LearnRateDropFactor',0.5,...
        'ValidationData',{{XTrain,YTrain}},...
        'ValidationFrequency',1,...
        'Verbose',1,...
        'Plots','none');
    
    net = trainNetwork(XTrain,YTrain,layers,options);
    [net,YPred] = predictAndUpdateState(net, XTrain, 'ExecutionEnvironment', 'gpu');
    YPred = sig*YPred+ mu;
    
    X(:,i) = YPred;
    
    % 计算误差指标
    RMSE(i) = sqrt(mean((X(:,i) - x(:,i)).^2));
    MSE(i) = mean((X(:,i) - x(:,i)).^2);
    MAE(i) = mean(abs(X(:,i) - x(:,i)));
    
    disp('预测结果评价指标：')
    disp(['RMSE = ', num2str(RMSE(i))])
    disp(['MSE  = ', num2str(MSE(i))])
    disp(['MAE  = ', num2str(MAE(i))])
end

figure
plot(t,x(1,:),'r',t,X(1,:),'*b','LineWidth',1);
legend('实际值','预测值')
xlabel('转子角度')
figure
plot(t,x(2,:),'r',t,X(2,:),'*b','LineWidth',1);
legend('实际值','预测值')
xlabel('转子角速度')
figure
plot(t,x(3,:),'r',t,X(3,:),'*b','LineWidth',1);
legend('实际值','预测值')
xlabel('转子转矩')
figure
plot(t,x(4,:),'r',t,X(4,:),'*b','LineWidth',1);
legend('实际值','预测值')
xlabel('转子d轴电流')
figure;
plot(t,x(5,:),'r',t,X(5,:),'*b','LineWidth',1);
legend('实际值','预测值')
xlabel('转子q轴电流')
figure
plot(t,x(6,:),'r',t,X(6,:),'*b','LineWidth',1);
legend('实际值','预测值')
xlabel('d电压')
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                    